
#include <math.h>

#include "Matrix44.h"

Matrix44::Matrix44()
{	
	_m[0][0]=1.f; _m[1][0]=0.f; _m[2][0]=0.f; _m[3][0]=0.f;
	_m[0][1]=0.f; _m[1][1]=1.f; _m[2][1]=0.f; _m[3][1]=0.f;
	_m[0][2]=0.f; _m[1][2]=0.f; _m[2][2]=1.f; _m[3][2]=0.f;
}
Matrix44::Matrix44(float m[4][3])
{
	_m[0][0]=m[0][0]; _m[1][0]=m[1][0]; _m[2][0]=m[2][0]; _m[3][0]=m[3][0]; 
	_m[0][1]=m[0][1]; _m[1][1]=m[1][1]; _m[2][1]=m[2][1]; _m[3][1]=m[3][1]; 
	_m[0][2]=m[0][2]; _m[1][2]=m[1][2]; _m[2][2]=m[2][2]; _m[3][2]=m[3][2]; 
}

float& Matrix44::operator() (unsigned int i, unsigned int j)
{
	return this->get(i, j);
}
float& Matrix44::get(unsigned int i, unsigned int j)
{
	if(i<4 && j<3)
		return this->_m[i][j];
	if(i<4 && j<4)
	{
		float f = (j==4 ? 1.f : 0.f);
		return f;
	}
	throw -1;
}

Vector4 Matrix44::operator *(Vector4 &v)
{
	Vector4 new_v;
	for(int i=0; i<3; i++)
	{
		float num = 0;
		for(int j=0; j<4; j++)
			num += this->_m[j][i] * v[j];
		new_v[i] = num;
	}
	new_v[3] = v[3];
	return new_v;
}

Matrix44 Matrix44::operator*(Matrix44 &mat)
{
	Matrix44 new_m = Matrix44();
	for(int rowA=0; rowA<3; rowA++)
	{
		for(int colB=0; colB<4; colB++)
		{
			float num = 0;
			for(int n=0; n<3; n++)
				num += _m[n][rowA] * mat(colB,n);
			num += _m[3][rowA] * (colB==3 ? 1.f : 0.f);
			new_m(colB,rowA) = num;
		}
	}
	return new_m;
}

Matrix44 Matrix44::times(Matrix44 *mat)
{
	Matrix44 new_m = Matrix44();
	for(int rowA=0; rowA<3; rowA++)
	{
		for(int colB=0; colB<4; colB++)
		{
			float num = 0;
			for(int n=0; n<3; n++)
				num += _m[n][rowA] * mat->get(colB,n);
			num += _m[3][rowA] * (colB==3 ? 1.f : 0.f);
			new_m(colB,rowA) = num;
		}
	}
	return new_m;
} Matrix44 Matrix44::times(Matrix44 &mat)
{
	Matrix44 new_m = Matrix44();
	for(int rowA=0; rowA<3; rowA++)
	{
		for(int colB=0; colB<4; colB++)
		{
			float num = 0;
			for(int n=0; n<3; n++)
				num += _m[n][rowA] * mat.get(colB,n);
			num += _m[3][rowA] * (colB==3 ? 1.f : 0.f);
			new_m(colB,rowA) = num;
		}
	}
	return new_m;
} Vector4 Matrix44::times(Vector4 &v)
{
	Vector4 new_v;
	for(int i=0; i<3; i++)
	{
		float num = 0;
		for(int j=0; j<4; j++)
			num += this->_m[j][i] * v[j];
		new_v[i] = num;
	}
	new_v[3] = v[3];
	return new_v;
}

Matrix44 Matrix44::inverse()
{
    //only built for a 4x4. Replace every element in the matrix with it's determinant,
    //transpose it, and then divide that matrix by the original matrix's determinant
    float a0 = get(0,0)*get(1,1) - get(0,1)*get(1,0);
    float a1 = get(0,0)*get(1,2) - get(0,2)*get(1,0);
    float a2 = get(0,0)*  0.f    -   0.f   *get(1,0);
	float a3 = get(0,1)*get(1,2) - get(0,2)*get(1,1);
	float a4 = get(0,1)*  0.f    -   0.f   *get(1,1);
	float a5 = get(0,2)*  0.f    -   0.f   *get(1,2);
	float b0 = get(2,0)*get(3,1) - get(2,1)*get(3,0);
	float b1 = get(2,0)*get(3,2) - get(2,2)*get(3,0);
	float b2 = get(2,0)*  1.f    -   0.f   *get(3,0);
	float b3 = get(2,1)*get(3,2) - get(2,2)*get(3,1);
	float b4 = get(2,1)*  1.f    -   0.f   *get(3,1);
	float b5 = get(2,2)*  1.f    -   0.f   *get(3,2);
	
    float det = a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;
	if(det != 0.f)
	{
		Matrix44 inv = Matrix44();
		inv.get(0,0) =  get(1,1)*b5 - get(1,2)*b4 +   0.f   *b3;
		inv.get(1,0) = -get(1,0)*b5 + get(1,2)*b2 -   0.f   *b1;
		inv.get(2,0) =  get(1,0)*b4 - get(1,1)*b2 +   0.f   *b0;
		inv.get(3,0) = -get(1,0)*b3 + get(1,1)*b1 - get(1,2)*b0;
		inv.get(0,1) = -get(0,1)*b5 + get(0,2)*b4 -   0.f   *b3;
		inv.get(1,1) =  get(0,0)*b5 - get(0,2)*b2 +   0.f   *b1;
		inv.get(2,1) = -get(0,0)*b4 + get(0,1)*b2 -   0.f   *b0;
		inv.get(3,1) =  get(0,0)*b3 - get(0,1)*b1 + get(0,2)*b0;
		inv.get(0,2) =  get(3,1)*a5 - get(3,2)*a4 +   1.f   *a3;
		inv.get(1,2) = -get(3,0)*a5 + get(3,2)*a2 -   1.f   *a1;
		inv.get(2,2) =  get(3,0)*a4 - get(3,1)*a2 +   1.f   *a0;
		inv.get(3,2) = -get(3,0)*a3 + get(3,1)*a1 - get(3,2)*a0;

        float invDet = 1.f/det;

		for(int i=0; i<16; i++)
			inv.get(i/4, i%4) *= invDet;

        return inv;
	}
    else{
        std::printf("ZERO - matrix invert error\n");
		throw -1;
	}
}



Matrix44 Matrix44::translateX(float x) { return translateXYZ(x,0,0); }
Matrix44 Matrix44::translateY(float y) { return translateXYZ(0,y,0); }
Matrix44 Matrix44::translateZ(float z) { return translateXYZ(0,0,z); }
Matrix44 Matrix44::translateXYZ(float x, float y, float z)
{
	Matrix44 retmat = Matrix44();
	retmat(4,0) = x;
	retmat(4,1) = y;
	retmat(4,2) = z;

	retmat = this->times(retmat);
	return retmat;
}

Matrix44 Matrix44::rotH(float h) { return rotHPR(h,0,0); }
Matrix44 Matrix44::rotP(float p) { return rotHPR(0,p,0); }
Matrix44 Matrix44::rotR(float r) { return rotHPR(0,0,r); }
Matrix44 Matrix44::rotHPR(float h, float p, float r) 
{
	Matrix44 retmat = Matrix44();
	retmat(0,0) = cos(h)*cos(p);
	retmat(1,0) = cos(h)*sin(p)*sin(r) - sin(h)*cos(r);
	retmat(2,0) = cos(h)*sin(p)*cos(r) + sin(h)*sin(r);
	retmat(3,0) = 0.f;

	retmat(0,1) = sin(h)*cos(p);
	retmat(1,1) = sin(h)*sin(p)*sin(r) + cos(h)*cos(r);
	retmat(2,1) = sin(h)*sin(p)*cos(r) - cos(h)*sin(r);
	retmat(3,1) = 0.f;

	retmat(0,2) = -sin(p);
	retmat(1,2) = cos(p)*sin(r);
	retmat(2,2) = cos(p)*cos(r);
	retmat(3,2) = 0.f;

	retmat = this->times(retmat);
	return retmat;
}

Matrix44 Matrix44::scale(float s) 
{
	Matrix44 retmat = Matrix44();
	retmat(0,0) = s;
	retmat(1,1) = s;
	retmat(2,2) = s;

	retmat = this->times(retmat);
	return retmat;
}

Matrix44 Matrix44::transformXYZHPRS(float x, float y, float z,
							   float h, float p, float r,
							   float s)
{
	Matrix44 retmat = Matrix44();
	
	return retmat;
}


void Matrix44::println()
{
	for(int i=0; i<3; i++)
		std::printf("%.3f %.3f %.3f %.3f\n", _m[0][i], _m[1][i], _m[2][i], _m[3][i]);
	std::printf("0.000 0.000 0.000 1.000\n");
	std::printf("\n");
}
